#include "aruco_test.h"
#include <memory>

using namespace std;

int main(int argc, char** argv)
{   
    ParameterReader pd;
    CameraConfig camera_conf;

    camera_conf.fx = atof(pd.getData("camera.fx").c_str());
    camera_conf.fy = atof(pd.getData("camera.fy").c_str());
    camera_conf.cx = atof(pd.getData("camera.cx").c_str());
    camera_conf.cy = atof(pd.getData("camera.cy").c_str());
    camera_conf.scale = atof(pd.getData("camera.scale").c_str());
    
    camera_conf.k1 = atof(pd.getData("camera.k1").c_str());
    camera_conf.k2 = atof(pd.getData("camera.k2").c_str());
    camera_conf.p1 = atof(pd.getData("camera.p1").c_str());
    camera_conf.p2 = atof(pd.getData("camera.p2").c_str());
    camera_conf.k3 = atof(pd.getData("camera.k3").c_str());

    camera_conf.initCameraConfig();

    int arucoFormatId = atoi(pd.getData("arucoFormatId").c_str());
    double markerLength = atof(pd.getData("markerLength").c_str());
    
    cout << "\n-------------\n" << endl;
    cout << "--camera.fx:\t" << camera_conf.fx << endl;
    cout << "--camera.fy:\t" << camera_conf.fy << endl;
    cout << "--camera.cx:\t" << camera_conf.cx << endl;
    cout << "--camera.cy:\t" << camera_conf.cy << endl;
    cout << "--camera.k1:\t" << camera_conf.k1 << endl;
    cout << "--camera.k2:\t" << camera_conf.k2 << endl;
    cout << "--camera.k3:\t" << camera_conf.k3 << endl;
    cout << "--camera.p1:\t" << camera_conf.p1 << endl;
    cout << "--camera.p2:\t" << camera_conf.p2 << endl;
    cout << "--arucoFormatId:\t" << arucoFormatId << endl;
    cout << "--markerLength:\t" << markerLength << endl;
    cout << "\n-------------\n" << endl;

    std::shared_ptr<aruco_node::ArucoTest> aruco_creator = std::make_shared<aruco_node::ArucoTest>(arucoFormatId);

    cv::VideoCapture cap(0);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
    cap.set(cv::CAP_PROP_FRAME_HEIGHT, 640);
    cap.set(cv::CAP_PROP_FPS, 30);

    cv::Mat frame;
    Result_Pose_estimation result;


    if(!cap.isOpened())
    {
        cout << "Please check out the number of video caputue." << endl;    
    }

    while(cap.isOpened())
    {
        cap >> frame;
        // aruco_creator->detete_aruco(frame);
        aruco_creator->pose_grid_board(frame, camera_conf, result);
        imshow("detet_aruco", frame);  
        char key = cv::waitKey(100);
        if(key == 'q')
            break;    
    }

    

    return 0;
}